import serial

from math import sqrt, ceil

ARM_RIGHT = 1
ARM_LEFT = 2
ROT_UP = 4
ROT_DOWN = 8
PEN_UP = 16
PEN_DOWN = 32

arm_edge = 260 #144*2
scale_factor = 1

def sign(x):
  return -1 if x < 0 else 0 if x == 0 else 1

class EggBot:
  def __init__(self):
    print "-----------EGGBOT CREATED!!"
    self.port = serial.Serial('/dev/ttyUSB0', 9600, timeout=1)
    self.arm = self.arm_act= 0.0
    self.spin = self.spin_act = 0.0

  def move(self, arm, spin):
    if arm == 0 and spin == 0:
      return

    arm, spin = scale_factor*arm, scale_factor*spin

    am = ARM_RIGHT if arm > 0 else 0 if arm == 0 else ARM_LEFT
    rt = ROT_UP if spin > 0 else 0 if spin == 0 else ROT_DOWN

    r = 1 if arm*arm + spin*spin <= 1 else sqrt(arm*arm + spin*spin)
    a0 = self.arm
    s0 = self.spin

    # Draw a straight line from our current position to the new position.
    # TODO: Output to the egg machine using parapin
    # TODO: Do a sanity check on arm_end and spin_end, coerce into agreeable range
    arm_end = self.arm + arm
    arm_increment = arm / r
    spin_end = self.spin + spin
    spin_increment = spin / r
    for i in range(1, ceil(r) + 1):
      arm_next = a0 + (min(i*arm_increment, arm) if arm >= 0 else max(i*arm_increment, arm))
      spin_next = s0 + (min(i*spin_increment, spin) if spin >= 0 else max(i*spin_increment, spin))
      print self.arm, self.spin, self.arm_act, self.spin_act, arm_next, spin_next
      if self.arm_act != round(arm_next) and self.spin_act != round(spin_next) and abs(round(arm_next)) <= arm_edge:
        print "arm", sign(arm_increment), "spin", sign(spin_increment)
        self.port.write(chr(rt|am))
        self.arm_act, self.spin_act = round(arm_next), round(spin_next)
      elif self.arm_act != round(arm_next) and abs(round(arm_next)) <= arm_edge:
        print "arm", sign(arm_increment)
        self.port.write(chr(am))
        self.arm_act = round(arm_next)
      elif self.spin_act != round(spin_next):
        print "spin", sign(spin_increment)
        self.port.write(chr(rt))
        self.spin_act = round(spin_next)
      else:
        print "NOTHING"
        self.port.write(chr(0))

      self.port.read()

      self.arm = arm_next
      self.spin = spin_next

  def move_absolute(self, arm, spin):
    # Use self.move to move us to the absolute position.
    self.move(arm - self.arm, spin - self.spin)

  def pen_up(self):
    self.port.write(chr(PEN_UP))
    self.port.read()

  def pen_down(self):
    self.port.write(chr(PEN_DOWN))
    self.port.read()

